/*
 * Kernel GDB tracepoint module.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 *
 * Copyright(C) Hui Zhu (teawater@gmail.com), 2010
 *
 * 2010-08-11	Demo release.  Just support X86-32 and collect $reg.
 * 2010-08-15	Support X86-64.
 * 2010-08-22	Support most X except env.
 */

#include <linux/version.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/proc_fs.h>
#include <linux/uaccess.h>
#include <linux/vmalloc.h>
#include <linux/kprobes.h>

#ifndef CONFIG_KPROBES
#error "Linux Kernel doesn't support KPROBES.  Please open it in 'General setup->Kprobes'."
#endif
#if !defined CONFIG_X86_32 && !defined CONFIG_X86_64
#error "KGTP support X86_32 and X86_64."
#endif

#if defined CONFIG_X86_32 || defined CONFIG_X86_64
#define ULONGEST		uint64_t
#define LONGEST			int64_t
#define CORE_ADDR		unsigned long
#endif
#ifdef CONFIG_X86_32
#define GTP_GDBRSP_REG_SIZE	128
#endif
#ifdef CONFIG_X86_64
#define GTP_GDBRSP_REG_SIZE	304
#endif

#define GTP_DEBUG		KERN_WARNING

#define GTP_RW_MAX		16384

#define GTP_FRAME_SIZE		5242880
#define GTP_FRAME_HEAD_SIZE	(1 + sizeof (struct gtp_frame_head))
#define GTP_FRAME_REG_SIZE	(1 + sizeof (struct gtp_frame_reg))
#define GTP_FRAME_MEM_SIZE	(1 + sizeof (struct gtp_frame_mem))

#define TOHEX(h)		((h) > 9 ?(h) + 'a' - 10 :(h) + '0')

struct action_agent_exp
{
	unsigned int	size;
	uint8_t		*buf;
};

struct action
{
	struct action	*next;
	char		type;
	union {
		ULONGEST		reg_mask;
		struct action_agent_exp	exp;
	} u;
};

struct gtp_entry
{
	struct gtp_entry	*next;
	ULONGEST		num;
	ULONGEST		addr;
	ULONGEST		step;
	ULONGEST		pass;
	int			nopass;
	int			kpreg;
	struct kprobe		kp;
	struct action		*action_list;
	struct action		*action_list_tail;
} *gtp_list = NULL, *gtp_list_tail = NULL;

struct gtp_frame_head
{
	ULONGEST	trace_num;
	char		*next;
};

struct gtp_frame_reg
{
	struct pt_regs	regs;
	char		*next;
};

struct gtp_frame_mem
{
	CORE_ADDR	addr;
	size_t		size;
	char		*next;
};

static atomic_t			gtp_count = ATOMIC_INIT(0);
static char 			gtp_read_ack = 0;
static char			*gtp_rw_buf = NULL;
static char			*gtp_rw_bufp = NULL;
static size_t			gtp_rw_size = 0;

static int			gtp_start = 0;

static int			gtp_disconnected_tracing = 0;
static int			gtp_circular = 0;

static DEFINE_SPINLOCK(gtp_frame_lock);
static char			*gtp_frame = NULL;
static char			*gtp_frame_r_start = NULL;
static char			*gtp_frame_w_start = NULL;
static char			*gtp_frame_w_end = NULL;
static int			gtp_frame_is_circular = 0;
static struct gtp_frame_head	*gtp_frame_current = NULL;

static char *
gtp_frame_alloc (size_t size)
{
	unsigned long	flags;
	char		*ret = NULL;

	if (size > GTP_FRAME_SIZE)
		return NULL;
	
	spin_lock_irqsave(&gtp_frame_lock, flags);

	if (gtp_frame_w_start + size > gtp_frame_w_end) {
		if (gtp_circular) {
			gtp_frame_is_circular = 1;
			gtp_frame_w_start = gtp_frame;
			gtp_frame_r_start = gtp_frame;
		}
		else
			goto out;
	}

	if (gtp_frame_is_circular) {
		/* Release some frame entry to get some place.
		   XXX:  When support new frame type, need add new handler
		   to switch.  */
		while (gtp_frame_w_start + size > gtp_frame_r_start) {
			switch (gtp_frame_r_start[0]) {
				case 'h':
					gtp_frame_r_start
						+= GTP_FRAME_HEAD_SIZE;
					break;
				case 'r':
					gtp_frame_r_start
						+= GTP_FRAME_REG_SIZE;
					break;
				case 'm': {
						struct gtp_frame_mem	*gfm;

						gfm = (struct gtp_frame_mem *)
							(gtp_frame_r_start + 1);
						gtp_frame_r_start
							+= GTP_FRAME_MEM_SIZE;
						gtp_frame_r_start += gfm->size;
					}
					break;
				default:
					goto out;
			}
		}
	}

	ret = gtp_frame_w_start;
	gtp_frame_w_start += size;

out:
	spin_unlock_irqrestore(&gtp_frame_lock, flags);
	return ret;
}

static ULONGEST
gtp_action_reg_read(struct pt_regs *regs, int num)
{
	ULONGEST	ret = 0;

#ifdef CONFIG_X86_32
	switch (num) {
		case 0:
			ret = regs->ax;
			break;
		case 1:
			ret = regs->cx;
			break;
		case 2:
			ret = regs->dx;
			break;
		case 3:
			ret = regs->bx;
			break;
		case 4:
			ret = regs->sp;
			break;
		case 5:
			ret = regs->bp;
			break;
		case 6:
			ret = regs->si;
			break;
		case 7:
			ret = regs->di;
			break;
		case 8:
			ret = regs->ip;
			break;
		case 9:
			ret = regs->flags;
			break;
		case 10:
			ret = regs->cs;
			break;
		case 11:
			ret = regs->ss;
			break;
		case 12:
			ret = regs->ds;
			break;
		case 13:
			ret = regs->es;
			break;
		case 14:
			ret = regs->fs;
			break;
		case 15:
			ret = regs->gs;
			break;
	}
#endif
#ifdef CONFIG_X86_64
	switch (num) {
		case 0:
			ret = regs->ax;
			break;
		case 1:
			ret = regs->bx;
			break;
		case 2:
			ret = regs->cx;
			break;
		case 3:
			ret = regs->dx;
			break;
		case 4:
			ret = regs->si;
			break;
		case 5:
			ret = regs->di;
			break;
		case 6:
			ret = regs->bp;
			break;
		case 7:
			ret = regs->sp;
			break;
		case 8:
			ret = regs->r8;
			break;
		case 9:
			ret = regs->r9;
			break;
		case 10:
			ret = regs->r10;
			break;
		case 11:
			ret = regs->r11;
			break;
		case 12:
			ret = regs->r12;
			break;
		case 13:
			ret = regs->r13;
			break;
		case 14:
			ret = regs->r14;
			break;
		case 15:
			ret = regs->r15;
			break;
		case 16:
			ret = regs->ip;
			break;
		case 17:
			ret = regs->flags;
			break;
		case 18:
			ret = regs->cs;
			break;
		case 19:
			ret = regs->ss;
			break;
	}
#endif

	return ret;
}

/* XXX: reg is ignore now.  */
static char **
gtp_action_memory_read (int reg, CORE_ADDR addr, size_t size, char **next)
{
	char			*tmp;
	struct gtp_frame_mem	*fm;

	tmp = gtp_frame_alloc(GTP_FRAME_MEM_SIZE + size);
	if (!tmp)
		return NULL;
	*next = tmp;

	tmp[0] = 'm';
	tmp ++;

	fm = (struct gtp_frame_mem *)tmp;
	tmp += sizeof (struct gtp_frame_mem);
	fm->addr = addr;
	fm->size = size;
	fm->next = NULL;

	if (probe_kernel_read (tmp, (void *)addr, size)) {
		memset (tmp, 0, size);
		printk(KERN_WARNING "gtp_action_memory_read: read %p %u "
				    "get error.\n", (void *)addr,
		       (unsigned int)size);
	}

	return &fm->next;
}

static char **
gtp_action_r(struct pt_regs *regs, struct action *ae, char **next)
{
	struct gtp_frame_reg	*freg;
	char			*tmp;

	tmp = gtp_frame_alloc(GTP_FRAME_REG_SIZE);
	if (!tmp)
		return NULL;

	*next = tmp;
	tmp[0] = 'r';
	freg = (struct gtp_frame_reg *) (tmp + 1);
	memcpy (&freg->regs, regs, sizeof (struct pt_regs));
#if !defined CONFIG_X86_32 && !defined CONFIG_X86_64
	freg->regs.sp = (unsigned long)&regs->sp;
#endif	/* CONFIG_X86_32 CONFIG_X86_64 */
	freg->next = NULL;

	return &freg->next;
}

static char **
gtp_action_x(struct pt_regs *regs, struct gtp_entry *tpe, struct action *ae, char **next)
{
	unsigned int	pc = 0, sp = 0;
	ULONGEST	top = 0;
	int		arg;
#define STACK_MAX	30
	ULONGEST	stack[STACK_MAX];
	union {
    		union {
			uint8_t	bytes[1];
      			uint8_t	val;
		} u8;
		union {
			uint8_t	bytes[2];
			uint16_t val;
		} u16;
		union {
			uint8_t bytes[4];
			uint32_t val;
		} u32;
		union {
			uint8_t bytes[8];
			ULONGEST val;
		} u64;
	} cnv;

	while (pc < ae->u.exp.size) {
#ifdef GTP_DEBUG
		printk(GTP_DEBUG "gtp_parse_x: cmd %x\n", ae->u.exp.buf[pc]);
#endif

		switch (ae->u.exp.buf[pc++]) {
			/* float */
			case 0x01:
				goto error_out;
				break;
			/* add */
			case 0x02:
				if (sp)
					top += stack[--sp];
				else
					goto error_out;
				break;
			/* sub */
			case 0x03:
				if (sp)
					top = stack[--sp] - top;
				else
					goto error_out;
				break;
			/* mul */
			case 0x04:
				if (sp)
					top *= stack[--sp];
				else
					goto error_out;
				break;
			/* div_signed */
			case 0x05:
				if (top && sp) {
					LONGEST l = (LONGEST) stack[--sp];
					do_div (l, (LONGEST) top);
					top = l;
				}
				else
					goto error_out;
				break;
			/* div_unsigned */
			case 0x06:
				if (top && sp) {
					ULONGEST ul = stack[--sp];
					do_div (ul, top);
					top = ul;
				}
				else
					goto error_out;
				break;
			/* rem_signed */
			case 0x07:
				if (top && sp) {
					LONGEST l1 = (LONGEST) stack[--sp];
					LONGEST l2 = (LONGEST) top;
					top = do_div (l1, l2);
				}
				else
					goto error_out;
				break;
			/* rem_unsigned */
			case 0x08:
				if (top && sp) {
					ULONGEST ul1 = stack[--sp];
					ULONGEST ul2 = top;
					top = do_div (ul1, ul2);
				}
				else
					goto error_out;
				break;
			/* lsh */
			case 0x09:
				if (sp)
					top = stack[--sp] << top;
				else
					goto error_out;
				break;
			/* rsh_signed */
			case 0x0a:
				if (sp)
					top = ((LONGEST) stack[--sp]) >> top;
				else
					goto error_out;
				break;
			/* rsh_unsigned */
			case 0x0b:
				if (sp)
					top = stack[--sp] >> top;
				else
					goto error_out;
				break;
			/* trace */
			case 0x0c:
				if (sp > 1) {
					next = gtp_action_memory_read
					           (-1, (CORE_ADDR) stack[--sp],
						    (size_t) top, next);
					if (!next)
						goto out;
					if (--sp >= 0)
						top = stack[sp];
				}
				else
					goto error_out;
				break;
			/* trace_quick */
			case 0x0d:
				if (pc >= ae->u.exp.size)
					goto error_out;
				next = gtp_action_memory_read
					   (-1, (CORE_ADDR) top,
					    (size_t) ae->u.exp.buf[pc++], next);
				if (!next)
					goto out;
				break;
			/* log_not */
			case 0x0e:
				top = !top;
				break;
			/* bit_and */
			case 0x0f:
				if (sp)
					top &= stack[--sp];
				else
					goto error_out;
				break;
			/* bit_or */
			case 0x10:
				if (sp)
					top |= stack[--sp];
				else
					goto error_out;
				break;
			/* bit_xor */
			case 0x11:
				if (sp)
					top ^= stack[--sp];
				else
					goto error_out;
				break;
			/* bit_not */
			case 0x12:
				top = ~top;
				break;
			/* equal */
			case 0x13:
				if (sp)
					top = (stack[--sp] == top);
				else
					goto error_out;
				break;
			/* less_signed */
			case 0x14:
				if (sp)
					top = (((LONGEST) stack[--sp])
						< ((LONGEST) top));
				else
					goto error_out;
				break;
			/* less_unsigned */
			case 0x15:
				if (sp)
					top = (stack[--sp] < top);
				else
					goto error_out;
				break;
			/* ext */
			case 0x16:
				if (pc >= ae->u.exp.size)
					goto error_out;
				arg = ae->u.exp.buf[pc++];
				if (arg < (sizeof (LONGEST) * 8)) {
					LONGEST mask = 1 << (arg - 1);
					top &= ((LONGEST) 1 << arg) - 1;
					top = (top ^ mask) - mask;
				}
				break;
			/* ref8 */
			case 0x17:
				if (probe_kernel_read
					(cnv.u8.bytes,
					 (void *)(CORE_ADDR)top, 1))
					goto error_out;
				top = (ULONGEST) cnv.u8.val;
				break;
			/* ref16 */
			case 0x18:
				if (probe_kernel_read
					(cnv.u16.bytes,
					 (void *)(CORE_ADDR)top, 1))
					goto error_out;
				top = (ULONGEST) cnv.u16.val;
				break;
			/* ref32 */
			case 0x19:
				if (probe_kernel_read
					(cnv.u32.bytes,
					 (void *)(CORE_ADDR)top, 1))
					goto error_out;
				top = (ULONGEST) cnv.u32.val;
				break;
			/* ref64 */
			case 0x1a:
				if (probe_kernel_read
					(cnv.u64.bytes,
					 (void *)(CORE_ADDR)top, 1))
					goto error_out;
				top = (ULONGEST) cnv.u64.val;
				break;
			/* ref_float */
			case 0x1b:
				goto error_out;
				break;
			/* ref_double */
			case 0x1c:
				goto error_out;
				break;
			/* ref_long_double */
			case 0x1d:
				goto error_out;
				break;
			/* l_to_d */
			case 0x1e:
				goto error_out;
				break;
			/* d_to_l */
			case 0x1f:
				goto error_out;
				break;
			/* if_goto */
			case 0x20:
				if (pc + 1 >= ae->u.exp.size)
					goto error_out;
				if (top)
					pc = (ae->u.exp.buf[pc] << 8)
						+ (ae->u.exp.buf[pc + 1]);
				else
					pc += 2;
				if (sp && --sp >= 0)
					top = stack[sp];
				break;
			/* goto */
			case 0x21:
				if (pc + 1 >= ae->u.exp.size)
					goto error_out;
				pc = (ae->u.exp.buf[pc] << 8)
					+ (ae->u.exp.buf[pc + 1]);
				break;
			/* const8 */
			case 0x22:
				if (pc >= ae->u.exp.size)
					goto error_out;
				stack[sp ++] = top;
				top = ae->u.exp.buf[pc++];
				break;
			/* const16 */
			case 0x23:
				if (pc + 1 >= ae->u.exp.size)
					goto error_out;
				stack[sp ++] = top;
				top = ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				break;
			/* const32 */
			case 0x24:
				if (pc + 3 >= ae->u.exp.size)
					goto error_out;
				stack[sp ++] = top;
				top = ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				break;
			/* const64 */
			case 0x25:
				if (pc + 7 >= ae->u.exp.size)
					goto error_out;
				stack[sp ++] = top;
				top = ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				top = (top << 8) + ae->u.exp.buf[pc++];
				break;
			/* reg */
			case 0x26:
				if (pc + 1 >= ae->u.exp.size)
					goto error_out;
				stack[sp++] = top;
				arg = ae->u.exp.buf[pc++];
				arg = (arg << 8) + ae->u.exp.buf[pc++];
				top = gtp_action_reg_read (regs, arg);
				break;
			/* end */
			case 0x27:
				goto out;
				break;
			/* dup */
			case 0x28:
				stack[sp++] = top;
				break;
			/* pop */
			case 0x29:
				if (sp && --sp >= 0)
					top = stack[sp];
				break;
			/* zero_ext */
			case 0x2a:
				if (pc >= ae->u.exp.size)
					goto error_out;
				arg = ae->u.exp.buf[pc++];
				if (arg < (sizeof (LONGEST) * 8))
					top &= ((LONGEST) 1 << arg) - 1;
				break;
			/* swap */
			case 0x2b:
				if (sp) {
					stack[sp] = top;
					top = stack[sp - 1];
					stack[sp - 1] = stack[sp];
				}
				else
					goto error_out;
				break;
			/* getv */
			case 0x2c:
				/* XXX */
				goto error_out;
				break;
			/* setv */
			case 0x2d:
				/* XXX */
				goto error_out;
				break;
			/* tracev */
			case 0x2e:
				/* XXX */
				goto error_out;
				break;
			/* trace16 */
			case 0x30:
				/* XXX */
				goto error_out;
				break;

			default:
				goto error_out;
				break;
		}

		if (sp > STACK_MAX - 5) {
			printk(KERN_WARNING "gtp_action_x: stack underflow.\n");
			goto error_out;
		}
	}
error_out:
	printk(KERN_WARNING "gtp_action_x: tracepoint %d "
			    "action X get error in pc %u.\n",
		(int)tpe->num, pc);
out:
	return next;
}

static int
gtp_kp_pre_handler(struct kprobe *p, struct pt_regs *regs)
{
	struct gtp_entry	*tpe = container_of(p, struct gtp_entry, kp);
	struct gtp_frame_head	*head;
	char			*tmp;
	struct action		*ae;
	char			**next;

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_kp_pre_handler: tracepoint %d\n",
		(int)tpe->num);
#endif

	/* Get the head.  */
	tmp = gtp_frame_alloc(GTP_FRAME_HEAD_SIZE);
	if (!tmp)
		goto no_memory;
	tmp[0] = 'h';
	head = (struct gtp_frame_head *) (tmp + 1);
	head->trace_num = tpe->num;
	head->next = NULL;
	next = &head->next;

	/* Handle actions.  */
	for (ae = tpe->action_list; ae; ae = ae->next) {
		switch (ae->type) {
			case 'R':
				next = gtp_action_r (regs, ae, next);
				if (!next)
					goto no_memory;
				break;
			case 'X':
				next = gtp_action_x (regs, tpe, ae, next);
				if (!next)
					goto no_memory;
				break;
		}
	}

	return 0;

no_memory:
	printk(KERN_WARNING "gtp_kp_pre_handler: tracepoint %d no memory.\n",
		(int)tpe->num);
	return 0;
}

static struct action *
gtp_action_alloc(char type)
{
	struct action	*ret;

	ret = kmalloc(sizeof(struct action), GFP_KERNEL);
	if (!ret)
		goto out;

	memset(ret, '\0', sizeof(struct action));
	ret->type = type;

out:
	return ret;
}

static void
gtp_action_release(struct action *ae)
{
	struct action	*ae2;

	while(ae) {
		ae2 = ae;
		ae = ae->next;
		/* Release ae2.  */
		switch (ae2->type) {
			case 'X':
				kfree(ae2->u.exp.buf);
				break;
		}
		kfree(ae2);
	}
}

static struct gtp_entry *
gtp_list_add(ULONGEST num, ULONGEST addr)
{
	struct gtp_entry	*ret = kmalloc(sizeof(struct gtp_entry),
					       GFP_KERNEL);

	if (!ret)
		goto out;
	memset(ret, '\0', sizeof(struct gtp_entry));
	ret->num = num;
	ret->addr = addr;
	ret->kp.addr = (kprobe_opcode_t *) (CORE_ADDR)addr;
	ret->kp.pre_handler = gtp_kp_pre_handler;

	/* Add to gtp_list.  */
	if (!gtp_list) {
		gtp_list = ret;
		gtp_list_tail = ret;
	}
	else {
		gtp_list_tail->next = ret;
		gtp_list_tail = ret;
	}

out:
	return ret;
}

static struct gtp_entry *
gtp_list_find(ULONGEST num, ULONGEST addr)
{
	struct gtp_entry	*tpe;

	for(tpe = gtp_list; tpe; tpe = tpe->next) {
		if (tpe->num == num && tpe->addr == addr)
			return tpe;
	}

	return NULL;
}

static void
gtp_list_release(void)
{
	struct gtp_entry	*tpe, *tpe2;

	tpe = gtp_list;
	while(tpe) {
		tpe2 = tpe;
		tpe = tpe->next;
		/* Release tpe2.  */
		gtp_action_release(tpe2->action_list);
		kfree(tpe2);
	}
	gtp_list = NULL;
	gtp_list_tail = NULL;
}

static void
gtp_frame_reset(void)
{
	gtp_frame_r_start = gtp_frame;
	gtp_frame_w_start = gtp_frame;
	gtp_frame_w_end = gtp_frame + GTP_FRAME_SIZE;
	gtp_frame_is_circular = 0;
	gtp_frame_current = NULL;
}

static int
hex2int(char hex, int *i)
{
	if ((hex >= '0') &&(hex <= '9')) {
		*i = hex - '0';
		return 1;
	}
	if ((hex >= 'a') &&(hex <= 'f')) {
		*i = hex - 'a' + 10;
		return 1;
	}
	if ((hex >= 'A') &&(hex <= 'F')) {
		*i = hex - 'A' + 10;
		return 1;
	}

	return 0;
}

static char *
hex2ulongest(char *pkg, ULONGEST *u64)
{
	int	i;

	*u64 = 0;
	while(hex2int(pkg[0], &i)) {
		pkg ++;
		*u64 =(*u64) << 4;
		*u64 |= i & 0xf;
	}

	return pkg;
}

static int
gtp_gdbrsp_qtinit(void)
{
	if (gtp_start)
		return -EBUSY;

	gtp_list_release();

	if (gtp_frame)
		gtp_frame_reset();

	return 0;
}

static int
gtp_parse_x(struct action *ae, char **pkgp)
{
	ULONGEST	size;
	int		ret = 0, i, h, l;
	char		*pkg = *pkgp;

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_parse_x: %s\n", pkg);
#endif

	if (pkg[0] == '\0') {
		ret = -EINVAL;
		goto out;
	}
	pkg = hex2ulongest(pkg, &size);
	if (pkg[0] != ',') {
		ret = -EINVAL;
		goto out;
	}
	ae->u.exp.size = (unsigned int)size;
	pkg ++;

	ae->u.exp.buf = kmalloc(ae->u.exp.size, GFP_KERNEL);
	if (!ae->u.exp.buf)
		return -ENOMEM;

	for (i = 0; i < ae->u.exp.size
		    && hex2int(pkg[0], &h) && hex2int(pkg[1], &l);
	     i++) {
#ifdef GTP_DEBUG
		printk(GTP_DEBUG "gtp_parse_x: %s %d %d\n", pkg, h, l);
#endif
		ae->u.exp.buf[i] = (h << 4) | l;
		pkg += 2;
#ifdef GTP_DEBUG
		printk(GTP_DEBUG "gtp_parse_x: %x\n", ae->u.exp.buf[i]);
#endif
	}

	if (i != ae->u.exp.size) {
		kfree(ae->u.exp.buf);
		ret = -EINVAL;
		goto out;
	}

out:
	*pkgp = pkg;
	return ret;
}

static int
gtp_gdbrsp_qtdp(char *pkg)
{
	int			addnew = 1;
	ULONGEST		num, addr;
	struct gtp_entry	*tpe;

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_gdbrsp_qtdp: %s\n", pkg);
#endif

	if (gtp_start)
		return -EBUSY;

	if (pkg[0] == '-') {
		pkg ++;
		addnew = 0;
	}

	/* Get num and addr.  */
	if (pkg[0] == '\0')
		return -EINVAL;
	pkg = hex2ulongest(pkg, &num);
	if (pkg[0] == '\0')
		return -EINVAL;
	pkg++;
	pkg = hex2ulongest(pkg, &addr);
	if (pkg[0] == '\0')
		return -EINVAL;
	pkg++;

	tpe = gtp_list_find(num, addr);
	if (addnew) {
		if (tpe)
			return -EINVAL;
		if (pkg[0] == 'D')
			return 0;
		if (pkg[0] == '\0')
			return -EINVAL;
		pkg++;

		tpe = gtp_list_add(num, addr);
		if (tpe == NULL)
			return -ENOMEM;

		/* Get step and pass.  */
		if (pkg[0] == '\0')
			return -EINVAL;
		pkg++;
		pkg = hex2ulongest(pkg, &tpe->step);
		if (pkg[0] == '\0')
			return -EINVAL;
		pkg++;
		pkg = hex2ulongest(pkg, &tpe->pass);
		if (tpe->pass == 0)
			tpe->nopass = 1;
	}
	else if (tpe) {
		/* Add action to tpe.  */
		int	step_action = 0;

		if (pkg[0] == 'S') {
			pkg ++;
			step_action = 1;
			/* XXX: Still not support step.  */
			return 1;
		}
		while(pkg[0]) {
			struct action	*ae;
			int		ret;

			switch(pkg[0]) {
				case 'R':
					/* XXX: reg_mask is ignore.  */
					ae = gtp_action_alloc(pkg[0]);
					if (!ae)
						return -ENOMEM;
					pkg ++;
					pkg = hex2ulongest(pkg,
							   &ae->u.reg_mask);
					break;
				case 'X':
					ae = gtp_action_alloc(pkg[0]);
					if (!ae)
						return -ENOMEM;
					pkg ++;
					ret = gtp_parse_x(ae, &pkg);
					if (ret < 0) {
						kfree(ae);
						return ret;
					}
					break;
				case '-':
					pkg ++;
					break;
				default:
					/* XXX: Not support.  */
					return 1;
			}

			if (ae) {
				/* Add ae to tpe.  */
				if (!tpe->action_list) {
					tpe->action_list = ae;
					tpe->action_list_tail = ae;
				}
				else {
					tpe->action_list_tail->next = ae;
					tpe->action_list_tail = ae;
				}
			}
		}
	}
	else
		return -EINVAL;

	return 0;
}

static int
gtp_gdbrsp_qtdisconnected(char *pkg)
{
	ULONGEST setting;

	if (gtp_start)
		return -EBUSY;
	if (pkg[0] == '\0')
		return -EINVAL;

	hex2ulongest(pkg, &setting);
	gtp_disconnected_tracing = (int)setting;

	return 0;
}

static int
gtp_gdbrsp_qtbuffer(char *pkg)
{
	if (strncmp ("circular:", pkg, 9) == 0) {
		ULONGEST setting;

		pkg += 9;
		if (pkg[0] == '\0')
			return -EINVAL;
		hex2ulongest(pkg, &setting);
		gtp_circular = (int)setting;

		return 0;
	}

	return 1;
}

static struct gtp_frame_head *
gtp_frame_head_find(int num)
{
	struct gtp_frame_head	*ret = NULL;
	char			*tmp;
	int			tfnum = 0;

	tmp = gtp_frame_r_start;
	while (tmp < gtp_frame_w_start) {
		switch (tmp[0]) {
			/* XXX:  When support new frame type, need add
			   new handler to switch.  */
			case 'h':
				if (tfnum == num) {
					ret = (struct gtp_frame_head *) (tmp + 1);
					goto out;
				}
				tfnum ++;
				tmp += GTP_FRAME_HEAD_SIZE;
				break;
			case 'r':
				tmp += GTP_FRAME_REG_SIZE;
				break;
			case 'm': {
					struct gtp_frame_mem	*gfm;

					gfm = (struct gtp_frame_mem *)
						(tmp + 1);
					tmp += GTP_FRAME_MEM_SIZE;
					tmp += gfm->size;
				}
				break;
			default:
				goto out;
		}
	}

out:
	return ret;
}

static int
gtp_gdbrsp_qtframe(char *pkg)
{
	if (gtp_start)
		return -EBUSY;

	if (strncmp(pkg, "pc:", 3) == 0)	/* XXX */
		return 1;
	else if (strncmp(pkg, "tdp:", 4) == 0)	/* XXX */
		return 1;
	else if (strncmp(pkg, "range:", 6) == 0)	/* XXX */
		return 1;
	else if (strncmp(pkg, "outside:", 8) == 0)	/* XXX */
		return 1;
	else {
		ULONGEST		num;
		struct gtp_frame_head	*ret;

		if (pkg[0] == '\0')
			return -EINVAL;
		hex2ulongest(pkg, &num);

#ifdef GTP_DEBUG
		printk(GTP_DEBUG "gtp_gdbrsp_qtframe: %d\n", (int) num);
#endif
		if (((int) num) < 0) {
			/* Return to current.  */
			gtp_frame_current = NULL;

			return 0;
		}
		ret = gtp_frame_head_find ((int) num);
		if (ret) {
			gtp_frame_current = ret;
			sprintf (gtp_rw_bufp, "F%xT%x",
				 (int) num,
				 (unsigned int) gtp_frame_current->trace_num);
			gtp_rw_size += strlen (gtp_rw_bufp);
			gtp_rw_bufp += strlen (gtp_rw_bufp);
		}
		else {
			strcpy(gtp_rw_bufp, "F-1");
			gtp_rw_bufp += 3;
			gtp_rw_size += 3;
		}
	}

	return 1;
}

static int
gtp_gdbrsp_qtstop(void)
{
	struct gtp_entry	*tpe;

	if (!gtp_start)
		return -EBUSY;

	for(tpe = gtp_list; tpe; tpe = tpe->next) {
		if (tpe->kpreg) {
			unregister_kprobe(&tpe->kp);
			tpe->kpreg = 0;
		}
	}

	gtp_start = 0;

	return 0;
}

static int
gtp_gdbrsp_qtstart(void)
{
	struct gtp_entry	*tpe;

	if (gtp_start)
		return -EBUSY;

	if (!gtp_frame) {
		gtp_frame = vmalloc(GTP_FRAME_SIZE);
		if (!gtp_frame)
			return -ENOMEM;

		gtp_frame_reset ();
	}

	for(tpe = gtp_list; tpe; tpe = tpe->next) {
		if (tpe->action_list) {
			int	ret = register_kprobe(&tpe->kp);
			if (ret < 0) {
				gtp_gdbrsp_qtstop();
				return ret;
			}
			tpe->kpreg = 1;
		}
	}

	gtp_start = 1;

	return 0;
}

static int
gtp_gdbrsp_QT(char *pkg)
{
	int	ret = 1;

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_gdbrsp_QT: %s\n", pkg);
#endif

	if (strcmp("init", pkg) == 0)
		ret = gtp_gdbrsp_qtinit();
	else if (strncmp("DP:", pkg, 3) == 0)
		ret = gtp_gdbrsp_qtdp(pkg + 3);
	else if (strncmp("Disconnected:", pkg, 13) == 0)
		ret = gtp_gdbrsp_qtdisconnected(pkg + 13);
	else if (strncmp("Buffer:", pkg, 7) == 0)
		ret = gtp_gdbrsp_qtbuffer(pkg + 7);
	else if (strncmp("Frame:", pkg, 6) == 0)
		ret = gtp_gdbrsp_qtframe(pkg + 6);
	else if (strcmp("Start", pkg) == 0)
		ret = gtp_gdbrsp_qtstart();
	else if (strcmp("Stop", pkg) == 0)
		ret = gtp_gdbrsp_qtstop();

	return ret;
}

static uint8_t	gtp_m_buffer[0xffff];

static int
gtp_gdbrsp_m(char *pkg)
{
	int		i;
	ULONGEST	addr, len;

	/* Get add and len.  */
	if (pkg[0] == '\0')
		return -EINVAL;
	pkg = hex2ulongest(pkg, &addr);
	if (pkg[0] != ',')
		return -EINVAL;
	pkg ++;
	pkg = hex2ulongest(pkg, &len);
	if (len == 0)
		return -EINVAL;
	len &= 0xffff;
	len = (ULONGEST) min ((int)((GTP_RW_MAX - 4 - gtp_rw_size) / 2),
				(int)len);

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_gdbrsp_m: addr = 0x%lx len = %d\n",
		(unsigned long) addr, (int) len);
#endif

	if (gtp_start || !gtp_frame_current) {
		if (probe_kernel_read (gtp_m_buffer, (void *)(CORE_ADDR)addr,
					(size_t)len))
			return -EFAULT;
	}
	else {
		char			*next;

		memset (gtp_m_buffer, 0, len);

		next = gtp_frame_current->next;
		while (next) {
			struct gtp_frame_reg	*fr;
			struct gtp_frame_mem	*mr;
			ULONGEST		cur_start, cur_end;
			uint8_t			*buf;

			switch (next[0]) {
				case 'r':
					fr = (struct gtp_frame_reg *) (next + 1);
					next = fr->next;
					break;
				case 'm':
					mr = (struct gtp_frame_mem *) (next + 1);
					buf = next + GTP_FRAME_MEM_SIZE;
					next = mr->next;
#ifdef GTP_DEBUG
					printk(GTP_DEBUG "gtp_gdbrsp_m: addr = 0x%lx size = %lu\n",
						(unsigned long) mr->addr, (unsigned long) mr->size);
#endif
					cur_start = max (((ULONGEST)mr->addr), addr);
					cur_end = min (((ULONGEST)mr->addr
								  + mr->size),
						       (addr + len));
#ifdef GTP_DEBUG
					printk(GTP_DEBUG "gtp_gdbrsp_m: start = 0x%lx end = %lx\n",
						(unsigned long) cur_start, (unsigned long) cur_end);
#endif
					if (cur_start < cur_end)
						memcpy (gtp_m_buffer,
							buf + cur_start - mr->addr,
							cur_end - cur_start);
					break;
				default:
					next = NULL;
					break;
			}
		}
	}

	for (i = 0; i < (int)len; i ++) {
		sprintf (gtp_rw_bufp, "%02x", gtp_m_buffer[i]);
		gtp_rw_bufp += 2;
		gtp_rw_size += 2;
	}

	return 1;
}

static int
gtp_gdbrsp_g (void)
{
	char			*next;
	struct gtp_frame_reg	*fr;
	struct gtp_frame_mem	*mr;

	if (GTP_RW_MAX - 4 - gtp_rw_size < GTP_GDBRSP_REG_SIZE)
		return -E2BIG;

	if (gtp_start || !gtp_frame_current)
		goto empty_out;

	/* Get the fr.  */
	fr = NULL;
	next = gtp_frame_current->next;
	while (next) {
		switch (next[0]) {
			case 'r':
				fr = (struct gtp_frame_reg *) (next + 1);
				goto check;
				break;
			case 'm':
				mr = (struct gtp_frame_mem *) (next + 1);
				next = mr->next;
				break;
			default:
				next = NULL;
				break;
		}
	}
check:
	if (!fr)
		goto empty_out;

#ifdef CONFIG_X86_32
#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_gdbrsp_g: ax = 0x%x\n",
		(unsigned int) fr->regs.ax);
	printk(GTP_DEBUG "gtp_gdbrsp_g: cx = 0x%x\n",
		(unsigned int) fr->regs.cx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: dx = 0x%x\n",
		(unsigned int) fr->regs.dx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: bx = 0x%x\n",
		(unsigned int) fr->regs.bx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: sp = 0x%x\n",
		(unsigned int) fr->regs.sp);
	printk(GTP_DEBUG "gtp_gdbrsp_g: bp = 0x%x\n",
		(unsigned int) fr->regs.bp);
	printk(GTP_DEBUG "gtp_gdbrsp_g: si = 0x%x\n",
		(unsigned int) fr->regs.si);
	printk(GTP_DEBUG "gtp_gdbrsp_g: di = 0x%x\n",
		(unsigned int) fr->regs.di);
	printk(GTP_DEBUG "gtp_gdbrsp_g: ip = 0x%x\n",
		(unsigned int) fr->regs.ip);
	printk(GTP_DEBUG "gtp_gdbrsp_g: flags = 0x%x\n",
		(unsigned int) fr->regs.flags);
	printk(GTP_DEBUG "gtp_gdbrsp_g: cs = 0x%x\n",
		(unsigned int) fr->regs.cs);
	printk(GTP_DEBUG "gtp_gdbrsp_g: ss = 0x%x\n",
		(unsigned int) fr->regs.ss);
	printk(GTP_DEBUG "gtp_gdbrsp_g: ds = 0x%x\n",
		(unsigned int) fr->regs.ds);
	printk(GTP_DEBUG "gtp_gdbrsp_g: es = 0x%x\n",
		(unsigned int) fr->regs.es);
	printk(GTP_DEBUG "gtp_gdbrsp_g: fs = 0x%x\n",
		(unsigned int) fr->regs.fs);
	printk(GTP_DEBUG "gtp_gdbrsp_g: gs = 0x%x\n",
		(unsigned int) fr->regs.gs);
#endif
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.ax));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.cx));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.dx));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.bx));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.sp));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.bp));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.si));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.di));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.ip));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.flags));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.cs));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.ss));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.ds));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.es));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.fs));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x", (unsigned int) swab32(fr->regs.gs));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
#endif	/* CONFIG_X86_32 */

#ifdef CONFIG_X86_64
#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_gdbrsp_g: ax = 0x%lx\n", fr->regs.ax);
	printk(GTP_DEBUG "gtp_gdbrsp_g: bx = 0x%lx\n", fr->regs.bx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: cx = 0x%lx\n", fr->regs.cx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: dx = 0x%lx\n", fr->regs.dx);
	printk(GTP_DEBUG "gtp_gdbrsp_g: si = 0x%lx\n", fr->regs.si);
	printk(GTP_DEBUG "gtp_gdbrsp_g: di = 0x%lx\n", fr->regs.di);
	printk(GTP_DEBUG "gtp_gdbrsp_g: bp = 0x%lx\n", fr->regs.bp);
	printk(GTP_DEBUG "gtp_gdbrsp_g: sp = 0x%lx\n", fr->regs.sp);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r8 = 0x%lx\n", fr->regs.r8);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r9 = 0x%lx\n", fr->regs.r9);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r10 = 0x%lx\n", fr->regs.r10);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r11 = 0x%lx\n", fr->regs.r11);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r12 = 0x%lx\n", fr->regs.r12);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r13 = 0x%lx\n", fr->regs.r13);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r14 = 0x%lx\n", fr->regs.r14);
	printk(GTP_DEBUG "gtp_gdbrsp_g: r15 = 0x%lx\n", fr->regs.r15);
	printk(GTP_DEBUG "gtp_gdbrsp_g: ip = 0x%lx\n", fr->regs.ip);
	printk(GTP_DEBUG "gtp_gdbrsp_g: flags = 0x%lx\n", fr->regs.flags);
	printk(GTP_DEBUG "gtp_gdbrsp_g: cs = 0x%lx\n", fr->regs.cs);
	printk(GTP_DEBUG "gtp_gdbrsp_g: ss = 0x%lx\n", fr->regs.ss);
#endif
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.ax));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.bx));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.cx));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.dx));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.si));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.di));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.bp));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.sp));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r8));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r9));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r10));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r11));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r12));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r13));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r14));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.r15));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%016lx", (unsigned long) swab64(fr->regs.ip));
	gtp_rw_bufp += 16;
	gtp_rw_size += 16;
	sprintf (gtp_rw_bufp, "%08x",
		 (unsigned int) swab32((unsigned int)fr->regs.flags));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x",
		 (unsigned int) swab32((unsigned int)fr->regs.cs));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
	sprintf (gtp_rw_bufp, "%08x",
		 (unsigned int) swab32((unsigned int)fr->regs.ss));
	gtp_rw_bufp += 8;
	gtp_rw_size += 8;
#endif	/* CONFIG_X86_64 */

	return 1;

empty_out:
	memset(gtp_rw_bufp, '0', GTP_GDBRSP_REG_SIZE);
	gtp_rw_bufp += GTP_GDBRSP_REG_SIZE;
	gtp_rw_size += GTP_GDBRSP_REG_SIZE;
	return 1;
}

static int
gtp_open(struct inode *inode, struct file *file)
{
	if (atomic_inc_return(&gtp_count) > 1) {
		atomic_dec(&gtp_count);
		return -EBUSY;
	}

	gtp_read_ack = 0;
	gtp_rw_buf = vmalloc(GTP_RW_MAX);
	if (!gtp_rw_buf)
		return -ENOMEM;

	return 0;
}

static int
gtp_release(struct inode *inode, struct file *file)
{
	vfree(gtp_rw_buf);

	/* XXX:  not handle gtp_disconnected_tracing.  */
	gtp_gdbrsp_qtstop();
	gtp_gdbrsp_qtinit();
	vfree(gtp_frame);
	gtp_frame = NULL;

	atomic_dec(&gtp_count);

	return 0;
}

#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35))
static int
gtp_ioctl(struct inode *inode, struct file *file,
	  unsigned int cmd, unsigned long arg)
{
#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_ioctl: %x\n", cmd);
#endif

	return 0;
}
#else
static long
gtp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_ioctl: %x\n", cmd);
#endif

	return 0;
}
#endif

static ssize_t
gtp_write(struct file *file, const char __user *buf, size_t size,
	  loff_t *ppos)
{
	char		*rsppkg = NULL;
	int		i, ret;
	unsigned char	csum = 0;

	size = min(size,(size_t) GTP_RW_MAX);
	if (copy_from_user(gtp_rw_buf, buf, size))
		return -EFAULT;

	if (gtp_rw_buf[0] == '+' || gtp_rw_buf[0] == '-'
	    || gtp_rw_buf[0] == '\3')
		goto out;

	/* Check format and crc and get the rsppkg.  */
	for(i = 0; i < size - 2; i ++) {
		if (rsppkg == NULL) {
			if (gtp_rw_buf[i] == '$')
				rsppkg = gtp_rw_buf + i + 1;
		}
		else {
			if (gtp_rw_buf[i] == '#')
				break;
			else {
				csum += gtp_rw_buf[i];
			}
		}
	}
	if (rsppkg && gtp_rw_buf[i] == '#') {
		/* Format is OK.  Check crc.  */
		unsigned char	c1, c2;

		gtp_rw_buf[i] = '\0';

		c1 = gtp_rw_buf[i+1];
		c2 = gtp_rw_buf[i+2];
		if (csum ==(c1 << 4) + c2) {
#ifdef GTP_DEBUG
			printk(GTP_DEBUG "gtp_write: crc error\n");
#endif
			gtp_read_ack = '-';
			goto out;
		}
	}
	else {
#ifdef GTP_DEBUG
		printk(GTP_DEBUG "gtp_write: format error\n");
#endif
		gtp_read_ack = '-';
		goto out;
	}
	gtp_read_ack = '+';

#ifdef GTP_DEBUG
	printk(GTP_DEBUG "gtp_write: %s\n", rsppkg);
#endif

	/* Handle rsppkg and put return to gtp_rw_buf.  */
	gtp_rw_buf[0] = '$';
	gtp_rw_bufp = gtp_rw_buf + 1;
	gtp_rw_size = 0;
	ret = 1;
	switch(rsppkg[0])
	{
		case '?':
			strcpy(gtp_rw_bufp, "S05");
			gtp_rw_bufp += 3;
			gtp_rw_size += 3;
			break;
		case 'g':
			ret = gtp_gdbrsp_g ();
			break;
		case 'm':
			ret = gtp_gdbrsp_m (rsppkg + 1);
			break;
		case 'Q':
			if (rsppkg[1] == 'T')
				ret = gtp_gdbrsp_QT(rsppkg + 2);
			break;
	}
	if (ret == 0) {
		strcpy(gtp_rw_bufp, "OK");
		gtp_rw_bufp += 2;
		gtp_rw_size += 2;
	}
	else if (ret < 0) {
		sprintf(gtp_rw_bufp, "E%02x", -ret);
		gtp_rw_bufp += 3;
		gtp_rw_size += 3;
	}

	gtp_rw_bufp[0] = '#';
	csum = 0;
	for(i = 1; i < gtp_rw_size + 1; i++)
		csum += gtp_rw_buf[i];
	gtp_rw_bufp[1] = TOHEX(csum >> 4);
	gtp_rw_bufp[2] = TOHEX(csum & 0x0f);
	gtp_rw_bufp = gtp_rw_buf;
	gtp_rw_size += 4;

out:
	return size;
}

static ssize_t
gtp_read(struct file *file, char __user *buf, size_t size,
	 loff_t *ppos)
{
	if (gtp_read_ack) {
		int err = put_user(gtp_read_ack, buf);
		if (err)
			return -err;
		gtp_read_ack = 0;
		return 1;
	}

	size = min(gtp_rw_size, size);

	if (copy_to_user(buf, gtp_rw_bufp, size))
		return -EFAULT;
	gtp_rw_bufp += size;
	gtp_rw_size -= size;

	return size;
}

static const struct file_operations proc_gtp_operations = {
	.owner		= THIS_MODULE,
	.open		= gtp_open,
	.release	= gtp_release,
#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,6,35))
	.ioctl		= gtp_ioctl,
#else
	.unlocked_ioctl	= gtp_ioctl,
	.compat_ioctl	= gtp_ioctl,
#endif
	.read		= gtp_read,
	.write		= gtp_write,
};

int init_module(void)
{
 	if (proc_create("gtp", S_IFIFO | S_IRUSR | S_IWUSR, NULL,
 			&proc_gtp_operations) == NULL)
 		return -ENOMEM;

	return 0;
}

void cleanup_module(void)
{
	remove_proc_entry("gtp", NULL);

	gtp_gdbrsp_qtstop();
	gtp_gdbrsp_qtinit();
	vfree(gtp_frame);
}

MODULE_AUTHOR("Hui Zhu");
MODULE_LICENSE("GPL");
